package nl.scalasim

import scala.collection.mutable.{HashMap, PriorityQueue, HashSet}

/**
 * Generic implementation of the A* algorithm.
 *
 * It returns the path with the lowest total cost. 
 *
 * @author Yigal Duppen
 * @since Oct 6, 2010
 *
 * @tparam N the node type to operate on
 * @param cost a function defining the cost between two adjacent nodes
 * @param h an admissible heuristic for guessing the cost between two arbitrary nodes
 * @param neighbours a function returning the neighbours of a specific node. 
 */
class AStar[N](cost: (N, N) => Int, h: (N, N) => Int, neighbours: N => Set[N]) extends PathFinder[N] {

  /**
   * The A* implementation.
   * http://en.wikipedia.org/wiki/A*_search_algorithm#Pseudocode
   */
  def path(start: N, goal: N): Option[Seq[N]] = {
    val gScore = new HashMap[N, Int]()
    val hScore = new HashMap[N, Int]()
    val fScore = new HashMap[N, Int]()

    def cmp(a: N, b: N): Boolean = fScore(a) > fScore(b)
    val ordering: Ordering[N] = Ordering.fromLessThan(cmp)

    val closed = new HashSet[N]()
    val open = new PriorityQueue[N]()(ordering)
    val cameFrom = new HashMap[N, N]()

    gScore(start) = 0
    hScore(start) = h(start, goal)
    fScore(start) = hScore(start)

    open += start

    while (!open.isEmpty) {
      val x: N = open.dequeue

      if (x == goal) {
        return Some(reconstruct(cameFrom, goal))
      }
      closed += x

      (neighbours(x) -- closed) foreach {
        y =>
          val tentativeG = gScore(x) + cost(x, y)
          if (!open.contains(y) || tentativeG < gScore(y)) {
            cameFrom(y) = x
            gScore(y) = tentativeG
            hScore(y) = h(y, goal)
            fScore(y) = gScore(y) + hScore(y)
            open += y
          }
      }
    }

    return None
  }

  private def reconstruct(cameFrom: HashMap[N, N], goal: N): Seq[N] = {
    if (cameFrom.contains(goal)) {
      reconstruct(cameFrom, cameFrom(goal)) :+ goal
    }
    else {
      List(goal)
    }
  }

}